/*
* (c) 2014 University of Applied Sciences, Karlsruhe
* Project "Segmentation of depth data of a plenoptic camera"
* summer semester 2014
*
* contour_fitting_3d.h
* Contains a class which fits 2D contours into 3D point cloud data.
*/

#ifndef _CONTOUR_FITTING_3D_H_
#define _CONTOUR_FITTING_3D_H_

#include <ostream>
#include <vector>

#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <pcl/common/distances.h>

#include "sgBase/geometrics/line_3d.h"

#include "sgMain/2d/contour_2d.h"
#include "sgMain/3d/contour_3d.h"

using namespace sgBase;
using namespace sgMain;

namespace sgExecution
{
	/**
	A class to fit 2D contour data, given by a set of lines, into a 3D world point cloud. In order to to this, this class requires the knowledge of the intrinsic
	camera parameters to project the 2D contours into the 3D space and to unproject the 3D point cloud into 2D image coordinates.
	*/
	class ContourFitting3D
	{
	private:
		std::vector<Contour2D> contours; /// The 2D contours on the flat image
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr depthCloudInCameraSpace; /// the 3D point data in camera space coordinates (after perspective projection)
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr depthCloudInWorldSpace; /// the 3D point data in real world coordinates. No perspective projection is applied here.
		const cv::Mat matrixOfIntrinsicParameters; /// the matrix of intrinsic paramters of the camera (focal length and image center in pixel-related units).

	public:
		/**
		Constructor.
		\param[in] contours2D List of the 2D contours which should be fitted into the 3D world.
		\param[in] depthCloud The depth cloud in the camera space.
		\param[in] intrinsics The matrix of intrinsic parameters.
		*/
		ContourFitting3D(const std::vector<Contour2D> &contours2D, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &depthCloud, const cv::Mat &intrinsics);

		/**
		Fits the 2D contours into 3D world coordinates using the fitting method given as argument.
		\param[in] fittingMethod The method to fit the 2D contours into the 3D world points.
		\param[in] radius The maximum distance of a depth point, projected to the camera image plane, is allowed to have from the line which is currently fitted.
		\return A list of fitted 3D contours.		
		*/
		std::vector<Contour3D> FitContours(Segmentation3DDepthFittingMethod fittingMethod, double radius);
	};
}

#endif